/*
 * Copyright (c) Thorben Linneweber and others
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the
 * "Software"), to deal in the Software without restriction, including
 * without limitation the rights to use, copy, modify, merge, publish,
 * distribute, sublicense, and/or sell copies of the Software, and to
 * permit persons to whom the Software is furnished to do so, subject to
 * the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

using System.Collections.Generic;
using Jitter2.LinearMath;

using SoftFloat;

namespace Jitter2.Collision.Shapes
{
	
	/// <summary>
	/// Represents a generic convex hull, similar to <see cref="ConvexHullShape"/>. The shape is
	/// implicitly defined by a point cloud. It is not necessary for the points to lie on the convex hull.
	/// For performance optimization, this shape should ideally be used for a small number of points (maximum
	/// of 20-30).
	/// </summary>
	public class PointCloudShape : RigidBodyShape
	{
	    private JBBox cachedBoundingBox;
	    private JMatrix cachedInertia;
	    private sfloat cachedMass;
	    private NVector3 cachedCenter;
	
	    private List<NVector3> vertices;
	    private NVector3 shifted;
	
	    /// <summary>
	    /// Initializes a new instance of the <see cref="PointCloudShape"/> class.
	    /// </summary>
	    /// <param name="vertices">
	    /// A list containing all vertices that define the convex hull. The list is referenced and should
	    /// not be modified after passing it to the constructor.
	    /// </param>
	    public PointCloudShape(List<NVector3> vertices)
	    {
	        this.vertices = vertices;
	        UpdateShape();
	    }
	
	    private PointCloudShape()
	    {
	        vertices = null!;
	    }
	
	    /// <summary>
	    /// Creates a copy of this shape. The underlying data structure is shared
	    /// among the instances.
	    /// </summary>
	    public PointCloudShape Clone()
	    {
	        PointCloudShape result = new()
	        {
	            vertices = vertices,
	            cachedBoundingBox = cachedBoundingBox,
	            cachedCenter = cachedCenter,
	            cachedInertia = cachedInertia,
	            cachedMass = cachedMass,
	            shifted = shifted
	        };
	        return result;
	    }
	
	    /// <summary>
	    /// Gets or sets the shift value for the shape. This property can be used when constructing a rigid
	    /// body that contains one or more shapes.
	    /// </summary>
	    public NVector3 Shift
	    {
	        get => shifted;
	        set
	        {
	            shifted = value;
	            UpdateShape();
	        }
	    }
	
	    public void UpdateShape()
	    {
	        CalculateMassInertia();
	        CalcInitBox();
	    }
	
	    public void CalculateMassInertia()
	    {
	        ShapeHelper.CalculateMassInertia(this, out cachedInertia, out cachedCenter, out cachedMass);
	    }
	
	    public override void CalculateMassInertia(out JMatrix inertia, out NVector3 com, out sfloat mass)
	    {
	        inertia = cachedInertia;
	        com = cachedCenter;
	        mass = cachedMass;
	    }
	
	    public override void CalculateBoundingBox(in NQuaternion orientation, in NVector3 position, out JBBox box)
	    {
	        NVector3 halfSize = (sfloat)0.5f * (cachedBoundingBox.Max - cachedBoundingBox.Min);
	        NVector3 center = (sfloat)0.5f * (cachedBoundingBox.Max + cachedBoundingBox.Min);
	
	        JMatrix ori = JMatrix.CreateFromQuaternion(orientation);
	        JMatrix.Absolute(in ori, out JMatrix abs);
	        NVector3.Transform(halfSize, abs, out NVector3 temp);
	        NVector3.Transform(center, orientation, out NVector3 temp2);
	
	        box.Max = temp;
	        NVector3.Negate(temp, out box.Min);
	
	        NVector3.Add(box.Min, position + temp2, out box.Min);
	        NVector3.Add(box.Max, position + temp2, out box.Max);
	    
	        // 考虑位置和中心偏移
	        box.Min += centerOffset;
	        box.Max += centerOffset;
		}
	
	    private void CalcInitBox()
	    {
	        NVector3 vec = NVector3.UnitX;
	        SupportMap(vec, out NVector3 res);
	        cachedBoundingBox.Max.X = res.X;
	
	        vec = NVector3.UnitY;
	        SupportMap(vec, out res);
	        cachedBoundingBox.Max.Y = res.Y;
	
	        vec = NVector3.UnitZ;
	        SupportMap(vec, out res);
	        cachedBoundingBox.Max.Z = res.Z;
	
	        vec = -NVector3.UnitX;
	        SupportMap(vec, out res);
	        cachedBoundingBox.Min.X = res.X;
	
	        vec = -NVector3.UnitY;
	        SupportMap(vec, out res);
	        cachedBoundingBox.Min.Y = res.Y;
	
	        vec = -NVector3.UnitZ;
	        SupportMap(vec, out res);
	        cachedBoundingBox.Min.Z = res.Z;
	    }
	
	    public override void SupportMap(in NVector3 direction, out NVector3 result)
	    {
	        sfloat maxDotProduct = sfloat.MinValue;
	        int maxIndex = 0;
	        sfloat dotProduct;
	
	        for (int i = 0; i < vertices.Count; i++)
	        {
	            dotProduct = NVector3.Dot(vertices[i], direction);
	            if (dotProduct > maxDotProduct)
	            {
	                maxDotProduct = dotProduct;
	                maxIndex = i;
	            }
	        }
	
	        result = vertices[maxIndex] + shifted;
	    
	        result += centerOffset;// 应用中心偏移
		}
	
	    public override void GetCenter(out NVector3 point)
	    {
	        point = cachedCenter;
	    }
	}
}
